#include "sdl_mvector.h"

mvector3f operator*(const matrix4x4f & m, const mvector3f & v)
{
    mvector3f ret;

    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++)
            ret[i] += m[i * 4 + j] * v[j];
    }

    ret[0] += m[12];

    ret[1] += m[13];
    ret[2] += m[14];
    return ret;
}

matrix4x4f columns(const mvector4f & a, const mvector4f & b, const mvector4f & c, const mvector4f & d)
{
    matrix4x4f ret;

    for (int i = 0; i < 4; i++) {
        ret[4 * i + 0] = a[i];
        ret[4 * i + 1] = b[i];
        ret[4 * i + 2] = c[i];
        ret[4 * i + 3] = d[i];
    }

    return ret;
}

matrix4x4f columns(const mvector3f & a, const mvector3f & b, const mvector3f & c)
{
    matrix4x4f ret;
    ret[15] = 1.0;

    for (int i = 0; i < 3; i++) {
        ret[4 * i + 0] = a[i];
        ret[4 * i + 1] = b[i];
        ret[4 * i + 2] = c[i];
    }

    return ret;
}

matrix4x4f rotation(const mvector3f & n, const float &a)
{
    float s = std::sin(a);
    float c = std::cos(a);
    matrix4x4f ret;
    ret.set(1. + (1. - c)*(n[0] * n[0] - 1.), -n[2] * s + (1. - c) * n[0] * n[1], n[1] * s + (1. - c) * n[0] * n[2], 0.,
            n[2] * s + (1. - c) * n[0] * n[1], 1. + (1. - c)*(n[1] * n[1] - 1.), -n[0] * s + (1. - c) * n[1] * n[2], 0.,
            -n[1] * s + (1. - c) * n[0] * n[2], n[0] * s + (1. - c) * n[1] * n[2], 1. + (1. - c)*(n[2] * n[2] - 1.), 0.,
            0., 0., 0., 1.
            );
    return ret;
}

matrix4x4f translation(const mvector3f & a)
{
    matrix4x4f ret;
    ret.id();
    ret[12] += a[0];
    ret[13] += a[1];
    ret[14] += a[2];
    return ret;

}

matrix4x4f scaling(const mvector3f & a)
{
    matrix4x4f ret;
    ret.id();
    ret[0] = a[0];
    ret[5] = a[1];
    ret[10] = a[2];
    return ret;
}

